/*
 * rviz_motion_state.cpp
 *
 *  Created on: 16.09.2013
 *      Author: alex
 */
#include <bullet_rviz_test/rviz_motion_state.h>
#include <ros/node_handle.h>
#include <visualization_msgs/Marker.h>

namespace bullet_rviz_test
{

RvizMotionState::RvizMotionState(const btVector3 & box_half_extents,
    const btTransform & world_transform, int32_t id) :
    box_half_extents_(box_half_extents), world_transform_(world_transform), id_(
        id)
{
  color_.r = 0;
  color_.g = 1;
  color_.b = 0;
  color_.a = 1;
  ros::NodeHandle nh;
  marker_publisher_ = nh.advertise<visualization_msgs::Marker>(
      "visualization_marker", 10);
}

RvizMotionState::~RvizMotionState()
{
}

void RvizMotionState::getWorldTransform(btTransform & world_transform) const
{
  world_transform = world_transform_;
}

void RvizMotionState::setWorldTransform(const btTransform & world_transform)
{
  world_transform_ = world_transform;
}

void RvizMotionState::publishMarker()
{
  if (ros::ok() && marker_publisher_)
  {
    visualization_msgs::Marker marker;

    marker.header.frame_id = "/my_frame";
    marker.header.stamp = ros::Time::now();

    marker.ns = "basic_shapes";
    marker.id = id_;
    marker.type = visualization_msgs::Marker::CUBE;
    marker.action = visualization_msgs::Marker::ADD;

    btVector3 position = world_transform_.getOrigin();
    marker.pose.position.x = position.x();
    marker.pose.position.y = position.y();
    marker.pose.position.z = position.z();

    btQuaternion orientation = world_transform_.getRotation();
    marker.pose.orientation.x = orientation.x();
    marker.pose.orientation.y = orientation.y();
    marker.pose.orientation.z = orientation.z();
    marker.pose.orientation.w = orientation.w();

    marker.scale.x = box_half_extents_.x() * 2;
    marker.scale.y = box_half_extents_.y() * 2;
    marker.scale.z = box_half_extents_.z() * 2;

    marker.color = color_;

    marker.lifetime = ros::Duration();

    marker_publisher_.publish(marker);
  }
}

void RvizMotionState::setColor(const std_msgs::ColorRGBA & color)
{
  color_ = color;
}

void RvizMotionState::setColor(uint32_t rgba)
{
  color_.r = ((rgba >> 24U) & 0xFFU) / 255.0f;
  color_.g = ((rgba >> 16U) & 0xFFU) / 255.0f;
  color_.b = ((rgba >> 8U) & 0xFFU) / 255.0f;
  color_.a = ((rgba >> 0U) & 0xFFU) / 255.0f;
}

}
